% Florian Meyer, 22/04/15.

function newParticles = kernelDensityResampling(weights, oldParticles, particlesNum, measurementVariance, smartResampling)
[~, stateLength] =  size(oldParticles);

if(smartResampling)
    
    %calculate variance of circularly symmetric Gaussian kernel functions
    particleMean(1) = sum(weights.*oldParticles(:,1));
    particleMean(2) = sum(weights.*oldParticles(:,2));
    particleVariance(1) = sum(weights.*oldParticles(:,1).^2) - particleMean(1)^2;
    particleVariance(2) = sum(weights.*oldParticles(:,2).^2) - particleMean(2)^2;
    kernelVariance = particlesNum^(-1/3) * particleVariance;
    
    if(mean(kernelVariance) > measurementVariance)
        kernelVariance = [measurementVariance measurementVariance];
    end
else
    kernelVariance = [0 0];
end
%perform systematic resampling
tmp = resampleSystematic(weights,particlesNum);
newParticles = oldParticles(tmp,1:2) + [sqrt(kernelVariance(1))*randn(particlesNum,1) sqrt(kernelVariance(2))*randn(particlesNum,1)];
if(stateLength == 4)
    newParticles(:,3:4) = oldParticles(tmp,3:4);
end

end